/**************************************************************************************
* @file   : bsp_drive_03.c
* @version: 
* @date   : 2025-01-07
* @file   : George
* @brief  : 
**************************************************************************************/

#include "bsp_drive_03.h"

uint8_t motor_enable_frame[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; /* 电机使能帧 */

int float_to_uint_03_1(float x, float x_min, float x_max, int bits){
  float span = x_max - x_min;
  float offset = x_min;
  if(x > x_max) x=x_max;
  else if(x < x_min) x= x_min;
  return (int) ((x-offset)*((float)((1<<bits)-1))/span);
}

static void motor_03_float_to_uint8_t(float f, uint8_t *bytes) {  
    union {  
        float f;  
        uint8_t b[sizeof(float)];  
    } converter;  
  
    converter.f = f;  
  
    // 假设我们是在大端字节序系统上  
    bytes[0] = converter.b[0]; // 最高有效字节  
    bytes[1] = converter.b[1];  
    bytes[2] = converter.b[2];  
    bytes[3] = converter.b[3]; // 最低有效字节  
}  

/**************************************************************************************
** @name:              电机使能
** @brief:             
** @param:             id:电机id, master_id:主控id, send_exid:发送拓展id, send_data:发送数据
** @retval:            
** @author:            George
** @date:              2025-01-13
** @version:           V0.0
**************************************************************************************/
void motor_enable(uint8_t id, uint16_t master_id,uint32_t * send_exid,uint8_t * send_data)
{
    (*((struct exCanIdInfo*)&(*send_exid))).mode = 3;
    (*((struct exCanIdInfo*)&(*send_exid))).id = id;
    (*((struct exCanIdInfo*)&(*send_exid))).res = 0;
    (*((struct exCanIdInfo*)&(*send_exid))).data = master_id;
    send_data[0] = 0x00;
    send_data[1] = 0x00;
    send_data[2] = 0x00;
    send_data[3] = 0x00;
    send_data[4] = 0x00;
    send_data[5] = 0x00;
    send_data[6] = 0x00;
    send_data[7] = 0x00;
}

/**************************************************************************************
** @name:              电机控制模式
** @brief:             
** @param:             id:电机id, torque:转矩, MechPosition:位置, speed:速度, kp:kp, kd:kd, send_exid:发送拓展id, send_data:发送数据
** @retval:            
** @author:            George
** @date:              2025-01-13
** @version:           V0.0
**************************************************************************************/
void motor_controlmode(uint8_t id, float torque, float MechPosition, 
float speed,float kp, float kd,uint32_t * send_exid,uint8_t * send_data)
{
    (*((struct exCanIdInfo*)&(*send_exid))).mode = 1;
    (*((struct exCanIdInfo*)&(*send_exid))).id = id;
    (*((struct exCanIdInfo*)&(*send_exid))).res = 0;
    (*((struct exCanIdInfo*)&(*send_exid))).data = float_to_uint_03_1(torque,T_MIN,T_MAX,16);

    send_data[0] = float_to_uint_03_1(MechPosition,P_MIN,P_MAX,16)>>8;
    send_data[1] = float_to_uint_03_1(MechPosition,P_MIN,P_MAX,16);
    send_data[2] = float_to_uint_03_1(speed,V_MIN,V_MAX,16)>>8;
    send_data[3] = float_to_uint_03_1(speed,V_MIN,V_MAX,16);
    send_data[4] = float_to_uint_03_1(kp,KP_MIN,KP_MAX,16)>>8;
    send_data[5] = float_to_uint_03_1(kp,KP_MIN,KP_MAX,16);
    send_data[6] = float_to_uint_03_1(kd,KD_MIN,KD_MAX,16)>>8;
    send_data[7] = float_to_uint_03_1(kd,KD_MIN,KD_MAX,16);
}

/**************************************************************************************
** @name:              电机复位
** @brief:             
** @param:             id:电机id, master_id:主控id, send_exid:发送拓展id, send_data:发送数据
** @retval:            
** @author:            George
** @date:              2025-01-13
** @version:           V0.0
**************************************************************************************/
void motor_reset(uint8_t id, uint16_t master_id,uint32_t * send_exid,uint8_t * send_data)
{
    (*((struct exCanIdInfo*)&(*send_exid))).mode = 4;
    (*((struct exCanIdInfo*)&(*send_exid))).id = id;
    (*((struct exCanIdInfo*)&(*send_exid))).res = 0;
    (*((struct exCanIdInfo*)&(*send_exid))).data = master_id;
    send_data[0] = 0x00;
    send_data[1] = 0x00;
    send_data[2] = 0x00;
    send_data[3] = 0x00;
    send_data[4] = 0x00;
    send_data[5] = 0x00;
    send_data[6] = 0x00;
    send_data[7] = 0x00;
}

/**************************************************************************************
** @name:              模式切换
** @brief:             
** @param:             id:电机id, master_id:主控id, runmode:运行模式, index:索引, send_exid:发送拓展id, send_data:发送数据
** @retval:            
** @author:            George
** @date:              2025-01-13
** @version:           V0.0
**************************************************************************************/
void motor_modechange(uint8_t id, uint16_t master_id, uint8_t *runmode,uint16_t *index,uint32_t * send_exid,uint8_t * send_data)
{
    (*((struct exCanIdInfo*)&(*send_exid))).mode = 0x12;
    (*((struct exCanIdInfo*)&(*send_exid))).id = id;
    (*((struct exCanIdInfo*)&(*send_exid))).res = 0;
    (*((struct exCanIdInfo*)&(*send_exid))).data = master_id;
    send_data[0] = 0x00;
    send_data[1] = 0x00;
    send_data[2] = 0x00;
    send_data[3] = 0x00;
    send_data[4] = 0x00;
    send_data[5] = 0x00;
    send_data[6] = 0x00;
    send_data[7] = 0x00;
    memcpy(&send_data[0],&index,2);
    memcpy(&send_data[4],&runmode, 1);
}


/**************************************************************************************
** @name:              电机写数据
** @brief:             
** @param:             id:电机id, master_id:主控id, ref:参考值, index:索引, send_exid:发送拓展id, send_data:发送数据
** @retval:            
** @author:            George
** @date:              2025-01-13
** @version:           V0.0
**************************************************************************************/
void motor_write(uint8_t id, uint16_t master_id, float *ref,uint16_t *index,uint32_t * send_exid,uint8_t * send_data)
{
    (*((struct exCanIdInfo*)&(*send_exid))).mode = 0x12;
    (*((struct exCanIdInfo*)&(*send_exid))).id = id;
    (*((struct exCanIdInfo*)&(*send_exid))).res = 0;
    (*((struct exCanIdInfo*)&(*send_exid))).data = master_id;
    send_data[0] = 0x00;
    send_data[1] = 0x00;
    send_data[2] = 0x00;
    send_data[3] = 0x00;
    send_data[4] = 0x00;
    send_data[5] = 0x00;
    send_data[6] = 0x00;
    send_data[7] = 0x00;
    memcpy(&send_data[0],&index,2);
    memcpy(&send_data[4],&ref,4);
}
/**************************************************************************************
** @name:              电机设置零点
** @brief:             
** @param:             
** @retval:            
** @author:            George
** @date:              2025-02-12
** @version:           V0.0
**************************************************************************************/
void motor_set_zero(uint8_t id, uint16_t master_id,uint32_t * send_exid,uint8_t * send_data)
{
    (*((struct exCanIdInfo*)&(*send_exid))).mode = 0x6;
    (*((struct exCanIdInfo*)&(*send_exid))).id = id;
    (*((struct exCanIdInfo*)&(*send_exid))).res = 0;
    (*((struct exCanIdInfo*)&(*send_exid))).data = master_id;
    send_data[0] = 0x01;
    send_data[1] = 0x01;
    send_data[2] = 0x00;
    send_data[3] = 0x00;
    send_data[4] = 0x00;
    send_data[5] = 0x00;
    send_data[6] = 0x00;
    send_data[7] = 0x00;
}

void motor03_data_to_can_data(float deg, uint8_t * send_data)
{
    uint8_t deg_float[4] = {0x00,0x00,0x00,0x00};
    motor_03_float_to_uint8_t(deg, deg_float);
    send_data[0] = 0x16;
    send_data[1] = 0x70;
    send_data[2] = 0x00;
    send_data[3] = 0x00;
    send_data[4] = deg_float[0];
    send_data[5] = deg_float[1];
    send_data[6] = deg_float[2];
    send_data[7] = deg_float[3];
}
